#!/bin/bash

# read -t 100 -p "Enter playback speed :" speed
# read -t 100 -p "Enter playback time :" time
# read -t 100 -p "Enter 0 to play all topics, 1 to play only /scan /odom /imu_data /map :" simulate

speed=$1
time=$2
simulate=$3

rosclean purge -y

DIR=$(pwd)

cd /mnt/HDD/home/ubuntu/BAG
roslaunch rviz_view.launch &

echo $DIR
cd $DIR

if [ $simulate -ne 1 ]; then
  rosrun rqt_console rqt_console &
  rosrun rqt_plot rqt_plot &
fi

function play_bag() {
  cd $DIR/home/ubuntu/log_bag

  if [ -f "last.bag" ] && [ -f "current.bag" ]; then
    echo "Playing last.bag and current.bag"
    if [ $3 -eq 1 ]; then
      echo "Playing specific topics: /scan /odom /imu_data /map"
      rosbag play last.bag current.bag --clock -r $1 -s $2 --topics /scan /odom /imu_data /map
    else
      echo "Playing all topics"
      rosbag play last.bag current.bag --clock -r $1 -s $2
    fi
  elif [ -f "last.bag" ]; then
    echo "Playing last.bag"
    if [ $3 -eq 1 ]; then
      echo "Playing specific topics: /scan /odom /imu_data /map"
      rosbag play last.bag --clock -r $1 -s $2 --topics /scan /odom /imu_data /map
    else
      echo "Playing all topics"
      rosbag play last.bag --clock -r $1 -s $2
    fi
  elif [ -f "current.bag" ]; then
    echo "Playing current.bag"
    if [ $3 -eq 1 ]; then
      echo "Playing specific topics: /scan /odom /imu_data /map"
      rosbag play current.bag --clock -r $1 -s $2 --topics /scan /odom /imu_data /map
    else
      echo "Playing all topics"
      rosbag play current.bag --clock -r $1 -s $2
    fi
  elif [ -f "last_unknow.bag" ] && [ -f "current_unknow.bag" ]; then
    echo "Playing last_unknow.bag and current_unknow.bag"
    if [ $3 -eq 1 ]; then
      echo "Playing specific topics: /scan /odom /imu_data /map"
      rosbag play last_unknow.bag current_unknow.bag --clock -r $1 -s $2 --topics /scan /odom /imu_data /map
    else
      echo "Playing all topics"
      rosbag play last_unknow.bag current_unknow.bag --clock -r $1 -s $2
    fi
  elif [ -f "last_unknow.bag" ]; then
    echo "Playing last_unknow.bag"
    if [ $3 -eq 1 ]; then
      echo "Playing specific topics: /scan /odom /imu_data /map"
      rosbag play last_unknow.bag --clock -r $1 -s $2 --topics /scan /odom /imu_data /map
    else
      echo "Playing all topics"
      rosbag play last_unknow.bag --clock -r $1 -s $2
    fi
  elif [ -f "current_unknow.bag" ]; then
    echo "Playing current_unknow.bag"
    if [ $3 -eq 1 ]; then
      echo "Playing specific topics: /scan /odom /imu_data /map"
      rosbag play current_unknow.bag --clock -r $1 -s $2 --topics /scan /odom /imu_data /map
    else
      echo "Playing all topics"
      rosbag play current_unknow.bag --clock -r $1 -s $2
    fi
  else
    echo "No bag file found in /home/ubuntu/log_bag directory."
    return 1
  fi
}

play_bag $speed $time $simulate
ok=true
while $ok
do	
  read -t 100 -p "Enter playback speed :" speed
  read -t 100 -p "Enter playback time :" time
  read -t 100 -p "Enter 0 to play all topics, 1 to play only /scan /odom /imu_data /map :" simulate
  play_bag $speed $time $simulate
done
